#include "World.h"
#include "PolyFactory.h"

/*
 *	World constructor
 */

World::World(Ogre::SceneManager *sceneManager)
{
	// get the scene manager
	m_sceneManager = sceneManager;

	// init game state
	m_gameState = RUNNING;
	m_targetHit = false;
	m_currentLevel = 1;
	
	//create polygon factory
	m_polyFactory = new PolyFactory(this);

	// init first level
	this->generateLevel();

	// create player and target sphere
	m_currentSphere = new Sphere(m_sceneManager, "SphereInit", MARBLE, "none", Ogre::Vector3(-150,40,-130), Ogre::Vector3(0,0,0), Ogre::Vector3(.2,.2,.2),20,TRUE);
	Sphere *target = new Sphere(m_sceneManager, "SphereTarget", MARBLE, "MMI/Target", Ogre::Vector3(80,120,-130), Ogre::Vector3(0,0,0), Ogre::Vector3(.2,.2,.2),20,TRUE);

	// add spheres to the collision vector
	m_sceneElements.push_back(m_currentSphere);
	m_sceneElements.push_back(target);
}

/*
 *	World destructor
 */

World::~World(void){}

/*
 *	World::worldStep is called by the base application on every new 'frame in'. It is used to drive the game.
 */

void World::worldStep(const Ogre::FrameEvent& evt)
{
	// check if there are any collisions (also calles move() on every elemnt in collision vector)
	this->checkWorldCollisions(evt);
	if(m_gameState == RESET)
	{
		m_gameState = RUNNING;
		reset();
	}
	if(m_targetHit && ((time(NULL)*1000 - m_initialTime) > 500))
	{
		m_targetHit = false;
		reset();
	}
}

/*
 *	World::createSphere can be used to generate a new sphere and add it to the scene and the collision vector
 */

void World::createSphere(Ogre::Vector3 initPosition, Ogre::Vector3 initSpeed)
{
	std::stringstream entityName;
	entityName << "Sphere_" << m_sceneElements.size() << std::endl;
	m_sceneElements.push_back(new Sphere(m_sceneManager, entityName.str(), MARBLE, "none", initPosition, initSpeed, Ogre::Vector3(.2,.2,.2),2,TRUE));
}

/*
 *	World::checkWorldColiisions checks for collisions between objects in the m_sceneElements vector (only works on speheres for now)
 */

void World::checkWorldCollisions(const Ogre::FrameEvent& evt)
{
	// compare each sphere with every other once
	for (int i = 0; i < m_sceneElements.size(); i++)
	{
		for (int j = i+1; j < m_sceneElements.size(); j++)
		{
			// get spheres from scene elements vector
			Sphere *sphereA = m_sceneElements.at(i);
			Sphere *sphereB = m_sceneElements.at(j);

			//check distance
			if(((sphereA->getPosition() - sphereB->getPosition()).length()) <= 40)
			{
				//compute frame lag compensation and adjust sphere position accordingly
				this->frameLagCompensation(sphereA, sphereB, evt);

				// collide spheres
				sphereA->collideWith(sphereB,evt);
				sphereB->collideWith(sphereA,evt);

				// save hit and record time
				m_targetHit = true;
				m_initialTime = time(NULL)*1000;
			}
		}

		// check for polygon collisions
		if(m_sceneElements.at(i)->getCollides()){
			this->checkPolyCollision(m_sceneElements.at(i), evt);
		}

		// check for environment collisions
		m_sceneElements.at(i)->collisionCheck();

		// call move on current sphere
		m_sceneElements.at(i)->move(evt);
	}
}

/*
 *	World::frameLagCompensation adjust collision radius of spehers upon impact to account for frame lag
 */

void World::frameLagCompensation(Sphere *sphereA, Sphere* sphereB,const Ogre::FrameEvent& evt)
{
	// get collision force transfer vector 
	Ogre::Vector3 center2Center = (sphereA->getPosition() - sphereB->getPosition());
	Ogre::Real c2cLength = center2Center.length() / 2;

	// normalize c2c vector (normalizes to length 1)
	Ogre::Vector3 normVector = center2Center / c2cLength;

	// compute lag compensation distance
	Ogre::Real lagCompensation = 20 - c2cLength; 

	// adjust sphere position according to lag compensation
	sphereA->setPosition(sphereA->getPosition() + (normVector * lagCompensation));
	sphereB->setPosition(sphereB->getPosition() + (normVector*-1 * lagCompensation));
}

void World::checkPolyCollision(Sphere *sphere, const Ogre::FrameEvent& evt)
{
	std::vector<Ogre::Vector2> collisionPoints;

	for (int i = 0; i < m_polygons.size(); i++)
	{
		//TOTO: Check for trivial non collision with polygon
		for (int j = 0; j < m_polygons.at(i)->m_polygonPoints.size()-1; j++)
		{
			// get line for sphere path
			Ogre::Vector2 dp1;
			dp1.x = sphere->getPreviousPosition().x;
			dp1.y = sphere->getPreviousPosition().y;

			Ogre::Vector2 dp2;
			dp2.x = sphere->getPosition().x;
			dp2.y = sphere->getPosition().y;

			//get current polygon edge
			Ogre::Vector2 poly1 = m_polygons.at(i)->m_polygonPoints.at(j);
			Ogre::Vector2 poly2 = m_polygons.at(i)->m_polygonPoints.at(j+1);

			bool trivial = false;

			// Check for trivial non collision through length
			Ogre::Vector3 polyCenter2D = Ogre::Vector3(m_polygons.at(i)->getPosition().x, m_polygons.at(i)->getPosition().y, -130);
			if(abs((sphere->getPosition() - polyCenter2D).length() > 100)){break;}

			//check trivial non collision
			if(((dp1.x > poly1.x && dp1.x > poly2.x) && (dp2.x > poly1.x && dp2.x > poly2.x)) ||
			   ((dp1.x < poly1.x && dp1.x < poly2.x) && (dp2.x < poly1.x && dp2.x < poly2.x)) ||
			   ((dp1.y > poly1.y && dp1.y > poly2.y) && (dp2.y > poly1.y && dp2.y > poly2.y)) ||
			   ((dp1.y < poly1.y && dp1.y < poly2.y) && (dp2.y < poly1.y && dp2.y < poly2.y))) {continue;}
			
			// compute intersection point
			Ogre::Vector2 intersect = Ogre::Vector2(-1000,-1000);
		    intersect = this->intersectsOnPoint(dp1, dp2, poly1, poly2);

			// save intersection in vector
			collisionPoints.push_back(intersect);

			// if intersection is detected
			if(intersect != Ogre::Vector2(-1000,-1000))
			{
				// frame lag compensation
				Ogre::Vector3 intersectPoint;
				intersectPoint.x = intersect.x;
				intersectPoint.y = intersect.y;
				intersectPoint.z = -130;

				sphere->setPosition(intersectPoint);

				// get normalized vector from polygon
				Ogre::Vector2 polygonVector = (poly2 - poly1)/((poly2 - poly1).length());

				//mirror displacement vector on polygonVector
				float dotproduct = (polygonVector.x * sphere->getDispVector().x) + (polygonVector.y * sphere->getDispVector().y);   
				
				Ogre::Vector3 mirror; 
				mirror.x = -1*(sphere->getDispVector().x - 2*polygonVector.x * dotproduct);
				mirror.y = -1*(sphere->getDispVector().y - 2*polygonVector.y * dotproduct);
				mirror.z = 0;

				// adjust dispalcement vector on sphere
				sphere->setAcceleration(mirror*sphere->getBounce());

				//move sphere off collision point by norm vector (avoid collision on next frame)
				mirror = mirror/mirror.length(); 
				sphere->setPosition(sphere->getPosition() + mirror);
				
				return;
			}
		}
	}
}

Ogre::Vector2 World::intersectsOnPoint(Ogre::Vector2 p0, Ogre::Vector2 p1, Ogre::Vector2 p2, Ogre::Vector2 p3)
{
	float s1_x, s1_y, s2_x, s2_y;
    s1_x = p1.x - p0.x;     
	s1_y = p1.y - p0.y;
    s2_x = p3.x - p2.x; 
	s2_y = p3.y - p2.y;

    float s, t;
    s = (-s1_y * (p0.x - p2.x) + s1_x * (p0.y - p2.y)) / (-s2_x * s1_y + s1_x * s2_y);
    t = ( s2_x * (p0.y - p2.y) - s2_y * (p0.x - p2.x)) / (-s2_x * s1_y + s1_x * s2_y);

    if (s >= 0 && s <= 1 && t >= 0 && t <= 1)
    {
        // collision detected, return collsion point
		return Ogre::Vector2(p0.x + (t * s1_x), p0.y + (t * s1_y));
    }
	// no collision detected, return std non collsision vector	
    return Ogre::Vector2(-1000,-1000); 
}

void World::addPolygonToWorld(Poly *polygon)
{
	m_polygons.push_back(polygon);
}

void World::drawPolygons()
{
	// for all polygons
	for(int i = 0; i < m_polygons.size(); i++)
	{
		// for all polygon sides
		Poly *current = m_polygons.at(i);
		for(int j = 0; j < current->m_polygonPoints.size()-1; j++)
		{
			// generate string to name manual object (must be unique)
			std::stringstream entityName;
			entityName << "manualObject" << i << "_" << j << std::endl;

			Ogre::ManualObject* myManualObject =  m_sceneManager->createManualObject(entityName.str()); 
			Ogre::SceneNode* myManualObjectNode = m_sceneManager->getRootSceneNode()->createChildSceneNode(entityName.str()); 

			Ogre::MaterialPtr myManualObjectMaterial = Ogre::MaterialManager::getSingleton().create("manual1Material",Ogre::ResourceGroupManager::DEFAULT_RESOURCE_GROUP_NAME); 
			myManualObjectMaterial->setReceiveShadows(false); 
			myManualObjectMaterial->getTechnique(0)->setLightingEnabled(true); 
			myManualObjectMaterial->getTechnique(0)->getPass(0)->setDiffuse(1,0,0,0); 
			myManualObjectMaterial->getTechnique(0)->getPass(0)->setAmbient(1,0,0); 
			myManualObjectMaterial->getTechnique(0)->getPass(0)->setSelfIllumination(1,0,0); 
			
			myManualObject->begin("manual1Material", Ogre::RenderOperation::OT_LINE_LIST); 
			myManualObject->position(current->m_polygonPoints.at(j).x, current->m_polygonPoints.at(j).y, -130); 
			myManualObject->position(current->m_polygonPoints.at(j+1).x, current->m_polygonPoints.at(j+1).y, -130); 
			myManualObject->end(); 
 
			myManualObjectNode->attachObject(myManualObject);
		}
	}
}

void World::setPhysics(int physicsComponent, Ogre::Real physicsValue)
{
	if(physicsComponent == GRAVITY)
	{
		m_sceneElements.at(0)->setGravity(physicsValue);

	}
	else if(physicsComponent == FRICTION)
	{
		m_sceneElements.at(0)->setFriction(physicsValue);
	}
	else if(physicsComponent == BOUNCE)
	{
		m_sceneElements.at(0)->setBounce(physicsValue);
	}
}

Ogre::Real World::getPhysics(int physicsComponent)
{
	if(physicsComponent == GRAVITY)
	{
		return m_currentSphere->getGravity();
	}
	else if(physicsComponent == FRICTION)
	{
		return m_currentSphere->getFriction();
	}
	else if(physicsComponent == BOUNCE)
	{
		return m_currentSphere->getBounce();
	}
	else
	{
		return -1000;
	}
}

void World::setGameState(int gameState)
{
	m_gameState = gameState;
}

int World::getGameState()
{
	return m_gameState;
}

void World::reset()
{
	//reset all spheres to their initial position
	for(int i = 0; i < m_sceneElements.size(); i++)
	{
		m_sceneElements.at(i)->setAcceleration(Ogre::Vector3(0,0,0));
		m_sceneElements.at(i)->setPreviousPosition(m_sceneElements.at(i)->getInitialPosition());
		m_sceneElements.at(i)->setPosition(m_sceneElements.at(i)->getInitialPosition());
	}
	if(m_currentLevel == 2){
		m_polygons.at(4)->setPosition(Ogre::Vector2(110,-180));
	}
}

void World::generateLevel(){

	// use factory to generate polygons
	m_polyFactory->createRectagle("collider001",Ogre::Vector2(-40,-180),-130,120,120,0,true,true,STATIC);
	m_polyFactory->createRectagle("collider002",Ogre::Vector2(-20,-100),-130,120,120,0,true,true,STATIC);

	m_polyFactory->createRectagle("collider003",Ogre::Vector2(40,50),-130,120,120,45,true,true,STATIC);
	m_polyFactory->createRectagle("collider004",Ogre::Vector2(-35,400),-130,120,120,0,true,true,STATIC);
	m_polyFactory->createRectagle("collider005",Ogre::Vector2(400,-180),-120, 120,120,0,true,true,STATIC);
	//this->drawPolygons();
}

void World::switchLevelOne(){
	m_sceneElements.at(1)->setInitialPosition(Ogre::Vector3(80,120,-130));
	this->reset();
	m_polygons.at(2)->setPosition(Ogre::Vector2(40,50));
	m_polygons.at(3)->setPosition(Ogre::Vector2(-35,400));
	m_polygons.at(4)->setPosition(Ogre::Vector2(400,-180));
	m_polygons.at(4)->makeDynamic(false);
}

void World::switchLevelTwo(){
	m_sceneElements.at(1)->setInitialPosition(Ogre::Vector3(175,-180,-130));
	this->reset();
	m_polygons.at(2)->setPosition(Ogre::Vector2(220,-140));
	m_polygons.at(3)->setPosition(Ogre::Vector2(-35,-20));
	m_polygons.at(4)->setPosition(Ogre::Vector2(110,-180));
	m_polygons.at(4)->makeDynamic(true);
}